#include <ros/ros.h>

#include <atomic>

#include<geometry_msgs/PoseStamped.h>
#include<geometry_msgs/TwistStamped.h>
#include<std_msgs/UInt64.h>
#include <std_msgs/Bool.h>
#include<sensor_msgs/PointCloud2.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
// #include <pcl/io/io.h>
// #include <pcl/io/pcd_io.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/common/common.h>
#include <pcl/filters/voxel_grid.h>

#include <octomap/octomap.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>

#include "unionsys_core.hpp"

#include"../SDPltUtils/SDPltBase.hpp"
#include "../SDPltDDS/SDPltPublisher.hpp"
#include "../SDPltDDS/SDPltSubscriber.hpp"

#include <fastdds/dds/publisher/Publisher.hpp>

#include "../SDPltDDSPose/SDPltPoseMsg.h"
#include "../SDPltDDSPose/SDPltPoseMsgPubSubTypes.h"
#include "../SDPltDDSPose/SDPltVelocityMsg.h"
#include "../SDPltDDSPose/SDPltVelocityMsgPubSubTypes.h"
#include "../SDPltDDSPose/SDPltStateMsg.h"
#include "../SDPltDDSPose/SDPltStateMsgPubSubTypes.h"
#include "../SDPltDDSPose/pointcloud2.h"
#include "../SDPltDDSPose/pointcloud2PubSubTypes.h"
//接收ros发布的topic,后dds再发布topic

ros::Subscriber tag_flag_sub;
//声明fastdds发布、ros的订阅类
SDPltPublisher<SDPltPoseMsgPubSubType, SDPltPoseMsg>* ddspub_pose=nullptr;
ros::Subscriber rossub_pose;

SDPltPublisher<SDPltVelocityMsgPubSubType, SDPltVelocityMsg>* ddspub_velocity=nullptr;
ros::Subscriber rossub_velocity;

SDPltPublisher<PointCloud2PubSubType, PointCloud2>* ddspub_pointcloud=nullptr;
ros::Subscriber rossub_pointcloud;

RegisterMSN *reg_pub = new RegisterMSN();


int drone_state1 = 0;
int drone_state2 = 0;
int drone_id;

double point_x = 0.0;
double point_y = 0.0;
double point_z = 0.0;

bool tag_flag_global = false;

//ros (mavros/local_position/pose) 回调函数
void rosposeloopdata(const geometry_msgs::PoseStamped::ConstPtr &pose){
    //使用new声明dds传输数据的类
    SDPltPoseMsg *PoseDDSMsg = new SDPltPoseMsg();

    //change child_frameid
    if(tag_flag_global)
    {
        drone_state1 = 1;
    }
    else{
        drone_state1 = 0;
    }
    std::string set_frame_id = reg_pub->serial_str(drone_state1, drone_state2);
    point_x = pose->pose.position.x;
    point_y = pose->pose.position.y;
    point_z = pose->pose.position.z;
    //赋值
    PoseDDSMsg->seq_(pose->header.seq);
    PoseDDSMsg->frame_id_(set_frame_id);
    PoseDDSMsg->xPoint_(pose->pose.position.x);
    PoseDDSMsg->yPoint_(pose->pose.position.y);
    PoseDDSMsg->zPoint_(pose->pose.position.z);
    PoseDDSMsg->xOrientation_(pose->pose.orientation.x);
    PoseDDSMsg->yOrientation_(pose->pose.orientation.y);
    PoseDDSMsg->zOrientation_(pose->pose.orientation.z);
    PoseDDSMsg->wOrientation_(pose->pose.orientation.w);

    bool suc = ddspub_pose->publish(*PoseDDSMsg);

     //释放new声明的类
    delete PoseDDSMsg;
}


//ros (mavros/local_position/velocity_local) 回调函数
void rosvelocityloopdata(const geometry_msgs::TwistStamped::ConstPtr &velocity){
    //使用new声明dds传输数据的类
    SDPltVelocityMsg *VelocityDDSMsg = new SDPltVelocityMsg();

    //赋值
    VelocityDDSMsg->seq_(velocity->header.seq);
    VelocityDDSMsg->frame_id_(velocity->header.frame_id);
    VelocityDDSMsg->xlinear_(velocity->twist.linear.x);
    VelocityDDSMsg->ylinear_(velocity->twist.linear.y);
    VelocityDDSMsg->zlinear_(velocity->twist.linear.z);
    VelocityDDSMsg->xangular_(velocity->twist.angular.x);
    VelocityDDSMsg->yangular_(velocity->twist.angular.y);
    VelocityDDSMsg->zangular_(velocity->twist.angular.z);

    ddspub_velocity->publish(*VelocityDDSMsg);

     //释放new声明的类
    delete VelocityDDSMsg;
}


void pc_callback(const sensor_msgs::PointCloud2::ConstPtr &points)
{
    if(!tag_flag_global)
    {
        std::cout << "no tag correct..will not publish pointclouds! " << std::endl;
        return;
    }
    //receive cloud message
    pcl::PointCloud<pcl::PointXYZ> latest_cloud1;
    pcl::fromROSMsg(*points, latest_cloud1);

    if (latest_cloud1.points.size() == 0)
    {   
        std::cout << "no cloud points" << std::endl;
        return;
    } 
    pcl::PointCloud<pcl::PointXYZ>::Ptr latest_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    latest_cloud = latest_cloud1.makeShared();
    //sample to voxel grid 
    // std::cout<<"Scene Point cloud size 1: "<<latest_cloud->points.size()<<std::endl;
    pcl::PCLPointCloud2::Ptr cloud_in2 (new pcl::PCLPointCloud2 ());
	pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
	toPCLPointCloud2(*latest_cloud,*cloud_in2);
	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
	sor.setInputCloud (cloud_in2);
	sor.setLeafSize(0.3,0.3,0.3);
	sor.filter (*cloud_filtered);
	fromPCLPointCloud2(*cloud_filtered,*latest_cloud);
    size_t pointsize = latest_cloud->points.size();
    std::cout << "------>pointsize:" << pointsize << std::endl;

    // pc_dds_msg->data(points->data);
    int size_temp = reg_pub->get_dds_pub_size();
    int send_times = (int)std::ceil((float)pointsize/(float)size_temp);

    for(int j = 0;j<send_times-1;j++)
    {
        PointCloud2 *pc_dds_msg = new PointCloud2();
    
        pc_dds_msg->point_x(point_x);
        pc_dds_msg->point_y(point_y);
        pc_dds_msg->point_z(point_z);
        for(size_t i = 0;i<size_temp;i++)
        {
            pc_dds_msg->data().push_back(latest_cloud->points[i+j*size_temp].x);
            pc_dds_msg->data().push_back(latest_cloud->points[i+j*size_temp].y);
            pc_dds_msg->data().push_back(latest_cloud->points[i+j*size_temp].z);
        }

        std::cout << "------>publish:" << j+1 << std::endl;
        pc_dds_msg->command(std::to_string(j+1));
        bool suc = ddspub_pointcloud->publish(*pc_dds_msg);
        delete pc_dds_msg;
    }
    //j=send_times-1
    PointCloud2 *pc_dds_msg = new PointCloud2();
    
    pc_dds_msg->point_x(point_x);
    pc_dds_msg->point_y(point_y);
    pc_dds_msg->point_z(point_z);
    int last_last = (int)pointsize%size_temp;
    for(size_t i = 0;i<last_last;i++)
    {
        pc_dds_msg->data().push_back(latest_cloud->points[i+(send_times-1)*size_temp].x);
        pc_dds_msg->data().push_back(latest_cloud->points[i+(send_times-1)*size_temp].y);
        pc_dds_msg->data().push_back(latest_cloud->points[i+(send_times-1)*size_temp].z);
    }

    std::cout << "------>publish_last:" << send_times << std::endl;
    pc_dds_msg->command(std::to_string(send_times));
    bool suc = ddspub_pointcloud->publish(*pc_dds_msg);
    

    delete pc_dds_msg;
}

void tag_flag_cb(const std_msgs::Bool::ConstPtr &msg)
{
    tag_flag_global = msg->data;
}


int main(int argc, char* argv[]){
    ros::init(argc, argv, "SDPltDDSPosepub");
    ros::NodeHandle nodeHandle; 
    reg_pub->detect_serial_number();

    // 设置循环的频率
    ros::Rate loop_rate(30);


    //从SDPltDDSPose.launch文件中获取dds、ros发布订阅topic的名字
    std::string ros_sub_Posetopic_name="", ros_sub_Velocitytopic_name="", dds_pub_Posetopic_name="", dds_pub_Velocitytopic_name="", ros_sub_pointcloudtopic_name="", dds_pub_pointcloudtopic_name="";
    nodeHandle.getParam("drone_id",drone_id);
    nodeHandle.getParam("ros_sub_Posetopic_name",ros_sub_Posetopic_name);
    nodeHandle.getParam("ros_sub_Velocitytopic_name",ros_sub_Velocitytopic_name);
    nodeHandle.getParam("ros_sub_pointcloudtopic_name",ros_sub_pointcloudtopic_name);

    nodeHandle.getParam("dds_pub_Posetopic_name",dds_pub_Posetopic_name);
    nodeHandle.getParam("dds_pub_Velocitytopic_name",dds_pub_Velocitytopic_name);
    nodeHandle.getParam("dds_pub_pointcloudtopic_name",dds_pub_pointcloudtopic_name);

    
    
    tag_flag_sub = nodeHandle.subscribe<std_msgs::Bool>("/tag_flag",10,tag_flag_cb);

    //先接收ros发布的topic,后dds再发布topic
    rossub_pose = nodeHandle.subscribe<geometry_msgs::PoseStamped>(ros_sub_Posetopic_name.c_str(),10,rosposeloopdata);
    ddspub_pose = new SDPltPublisher<SDPltPoseMsgPubSubType, SDPltPoseMsg>(dds_pub_Posetopic_name,"SDPltPoseMsg",BEST_EFFORT_RELIABILITY_QOS);
    rossub_velocity = nodeHandle.subscribe<geometry_msgs::TwistStamped>(ros_sub_Velocitytopic_name.c_str(),10,rosvelocityloopdata);
    ddspub_velocity = new SDPltPublisher<SDPltVelocityMsgPubSubType, SDPltVelocityMsg>(dds_pub_Velocitytopic_name,"SDPltVelocityMsg",BEST_EFFORT_RELIABILITY_QOS);

    rossub_pointcloud = nodeHandle.subscribe<sensor_msgs::PointCloud2>(ros_sub_pointcloudtopic_name.c_str(), 10, pc_callback);
    ddspub_pointcloud = new SDPltPublisher<PointCloud2PubSubType, PointCloud2>(dds_pub_pointcloudtopic_name,"PointCloud2",BEST_EFFORT_RELIABILITY_QOS);

    //publish trajectory
    // /planning_vis/trajectory /planning/travel_traj /planning/position_cmd_vis

    //循环回调
    while(ros::ok()&&nodeHandle.ok()){        
        ros::spinOnce();
        loop_rate.sleep();
    }
    // ros::spin();


    nodeHandle.shutdown();
    return 0;
}
